#include "mainwindow.h"
#include "ui_mainwindow.h"

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);
//    string str="127.0.0.1:5080";
    string str=ui->lineEdit_10->text().toStdString();
    rosCommnicate=new RosCommunicate(str);

    timer = new QTimer();
    connect(timer,SIGNAL(timeout()),this,SLOT(timeroutslot()));//timeoutslot()为自定义槽
    timer->start(300);

    joyStick= new BallJoyStick(400,this);
    useJoyStick=false;

}

MainWindow::~MainWindow()
{
    delete ui;
}

void MainWindow::on_pushButton_clicked()
{
    UserInfo userInfo;
    int robotIdIn=1;
    double xMove=ui->lineEdit_7->text().toDouble();
    double yMove=ui->lineEdit_8->text().toDouble();
    double rotate=ui->lineEdit_9->text().toDouble();
    int returnFlag;
    if(1==rosCommnicate->vehicleAuto(userInfo,robotIdIn,xMove,yMove,rotate,returnFlag))
    {

    }
    else
    {

    }
}

void MainWindow::timeroutslot()
{
    UserInfo userInfo;
    int robotIdIn=1;
    OdemeterData odemeterDataReturn;

    if(1==rosCommnicate->getOdometer(userInfo,robotIdIn,odemeterDataReturn))
    {
        ui->lineEdit->setText(QString::number(odemeterDataReturn.pos.x_move,'f',2));
        ui->lineEdit_2->setText(QString::number(odemeterDataReturn.pos.y_move,'f',2));
        ui->lineEdit_3->setText(QString::number(odemeterDataReturn.pos.z_rotate,'f',2));

        ui->lineEdit_4->setText(QString::number(odemeterDataReturn.vel.x_move,'f',2));
        ui->lineEdit_5->setText(QString::number(odemeterDataReturn.vel.y_move,'f',2));
        ui->lineEdit_6->setText(QString::number(odemeterDataReturn.vel.z_rotate,'f',2));
    }


    //
    std::vector<double> wheelSpeedReturn;
    if(1==rosCommnicate->getWheelMoveSpeed(userInfo,robotIdIn,wheelSpeedReturn))
    {
        ui->lineEdit_14->setText(QString::number(wheelSpeedReturn[0],'f',2));
        ui->lineEdit_15->setText(QString::number(wheelSpeedReturn[1],'f',2));

    }

    std::vector<double> wheelPathLengthReturn;
    if(1==rosCommnicate->getWheelPathLength(userInfo,robotIdIn,wheelPathLengthReturn))
    {
        ui->lineEdit_16->setText(QString::number(wheelPathLengthReturn[0],'f',2));
        ui->lineEdit_17->setText(QString::number(wheelPathLengthReturn[1],'f',2));

    }


    //joy stick control
    if(useJoyStick)
    {
        vehicleJog();
    }


}

void MainWindow::on_pushButton_2_clicked()
{
    joyStick->move(0,0);
    joyStick->show();
    useJoyStick=true;
}

void MainWindow::on_pushButton_3_clicked()
{
    joyStick->hide();
    useJoyStick=false;
}


int MainWindow::vehicleJog()
{

    UserInfo userInfo;
    int robotIdIn=1;

    double xMove=ui->lineEdit_11->text().toDouble();
    double yMove=ui->lineEdit_12->text().toDouble();
    double rotate=ui->lineEdit_13->text().toDouble();
    int returnFlag;
    if(1==rosCommnicate->vehicleAuto(userInfo,robotIdIn,xMove*joyStick->getY(),yMove,-rotate*joyStick->getX(),returnFlag))
    {

    }
    else
    {

    }

}
